// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Talina Gaming System (TgS) (∂)
//  »File«      TgS Common - Math API [Polynomial].c_inc
//  »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
//  »Version«   4.0
// ------------------------------------------------------------------------------------------------------------------------------ //
//  Copyright: © 2002-2010, Andrew Aye.  All Rights Reserved.
//  This software is free for non-commercial use. Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met: 
//    Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. 
//    Redistributions in binary form must reproduce this copyright notice, this list of conditions and the following
//      disclaimers in the documentation and other materials provided with the distribution. 
//  Neither the names of the copyright owner nor the names of its contributors may be used to endorse or promote products derived
//  from this software without specific prior written permission. 
//  The intellectual property rights of the algorithms used reside with Andrew Aye.  You may not use this software, in whole or
//  in part, in support of any commercial product without the express written consent of the author.
//  There is no warranty or other guarantee of fitness of this software for any purpose. It is provided solely "as is".
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //


// START TGS - MATH ////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// ---- Van Wijngaarden-Dekker-Brent Root Finder -------------------------------------------------------------------------------- //
//
//   Guaranteed to find a root to the equation given that the two initial parameters stradle a solution point (ie. one positive
//  and one negative value) - IVT guarantees at least one theoretical solution.  Iterative technique will refine solution until
//  its within the passed in tolerance value.  Implementation from Numerical Recipes.
//
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL F(F_BrentZ)( PCU_TYPE ptyT0, TYPE (*pfnFunc) (const TYPE), TYPE tyTA, TYPE tyTC )
{
    TYPE                                tyValA = (*pfnFunc)(tyTA);
    TYPE                                tyValC = (*pfnFunc)(tyTC);
    TYPE                                tyTB = tyTC;
    TYPE                                tyValB,p,q,d,e;
    TgSINT                              iCount;

    if (tyValA*tyValC > MKL(0.0))
    {
        // Root must be bracketed for IVT to be valid.

        return (TgFALSE);
    };

    tyValB = tyValC;
    tyValC = tyValA;
    tyTC = tyTA;
    e = d = tyTB - tyTA;

    for (iCount = 1; iCount <= 100; ++iCount)
    {
        TYPE                                tyTol1, tyTM;

        if (F(tgPM_ABS)(tyValC) < F(tgPM_ABS)(tyValB))
        {
            tyTA = tyTB;
            tyTB = tyTC;
            tyTC = tyTA;

            tyValA = tyValB;
            tyValB = tyValC;
            tyValC = tyValA;
        }

        // Convergence check.

        tyTol1 = MKL(2.0)*F(TgROOT_EPS)*F(tgPM_ABS)( tyTB ) + MKL(0.5)*F(TgROOT_EPS);
        tyTM = MKL(0.5)*(tyTC - tyTB);

        if (F(tgPM_ABS)(tyTM) <= tyTol1 || tyValB == MKL(0.0))
        {
            *ptyT0 = tyTB;
            return (TgTRUE);
        };

        if (F(tgPM_ABS)( e ) >= tyTol1 && F(tgPM_ABS)( tyValA ) > F(tgPM_ABS)( tyValB ))
        {
            // Attempt inverse quadratic interpolation.

            const TYPE                          tyBA = tyValB/tyValA, tyBC = tyValB/tyValC, tyAC = tyValA/tyValC;

            p = F(tgPM_FSEL)( -F(tgPM_ABS)(tyTA - tyTC), MKL(2.0)*tyTM*tyBA,
                tyBA*(MKL(2.0)*tyTM*tyAC*(tyAC - tyBC) - (tyTB - tyTA)*(tyBC - MKL(1.0)))
            );
            q = F(tgPM_FSEL)( -F(tgPM_ABS)(tyTA - tyTC), MKL(1.0) - tyBA, (tyAC - MKL(1.0))*(tyBC - MKL(1.0))*(tyBA - MKL(1.0)) );

            // Check whether in bounds.

            q = F(tgPM_FSEL)( p, -q, q );
            p = F(tgPM_ABS)( p );

            {
                const TYPE                          tyMin1 = MKL(3.0)*tyTM*q - F(tgPM_ABS)(tyTol1*q);
                const TYPE                          tyMin2 = F(tgPM_ABS)(e*q);
                const TYPE                          tyInt = F(tgPM_FSEL)( tyMin2 - tyMin1, tyMin1, tyMin2 ) - MKL(2.0)*p;

                e = F(tgPM_FSEL)( tyInt, d, tyTM );
                d = F(tgPM_FSEL)( tyInt, p/q, tyTM );
            }
//          P::FSEL( P::FSEL( tyMin2 - tyMin1, tyMin1, tyMin2 ) - TYPE(2.0)*p, Accept interpolation, Use bisection );
        }
        else
        {
            // Bounds decreasing too slowly, use bisection.

            d = tyTM;
            e = tyTM;
        }

        // Move last best guess to tyTA.

        tyTA = tyTB;
        tyValA = tyValB;

        // Evaluate new trial root.

        tyTB +=  F(tgPM_FSEL)( F(tgPM_ABS)(d) - tyTol1, d, F(tgPM_FSEL)( tyTM, tyTol1, -tyTol1 ) );
        tyValB = (*pfnFunc)(tyTB);

        if (tyValB*tyValC > MKL(0.0))
        {
            // Rename tyTA, tyTB, tyTC and adjust bounding interval

            tyTC = tyTA; 
            tyValC = tyValA;
            e = d = tyTB - tyTA;
        }
    }

    return (TgFALSE);
}


// ---- Van Wijngaarden-Dekker-Brent Root Finder -------------------------------------------------------------------------------- //
//
//  Given a function fn, and given a bracketing triplet of abscissas t1, t2, t3 (such that t2 is between t1 and t3, and f(t1) is
// less than both f(t1) and f(t3)), this routine isolates the minimum to a fractional precision of about tol using Brent's method.
// See Numerical Recipes in C, section 10.2
//
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL F(F_BrentD)(
    PCU_TYPE ptyT0, PCU_TYPE ptyV0, TYPE (*pfnFunc0) (const TYPE), TYPE (*pfnFunc1) (const TYPE), TYPE tyT1, TYPE tyT2, TYPE tyT3
) {
    TYPE                                tyTM, tyD = 0.0, tyE = 0.0;
    TYPE                                tyU, tyFU, tyDU;
    TYPE                                tyV, tyFV, tyDV;
    TYPE                                tyW, tyFW, tyDW;
    TYPE                                tyX, tyFX, tyDX;
    TYPE                                tyA = F(tgPM_FSEL)(tyT3 - tyT1, tyT1, tyT3);
    TYPE                                tyB = F(tgPM_FSEL)(tyT1 - tyT3, tyT1, tyT3);
    TgSINT32                            niCount;

    tyX  = tyW  = tyV  = tyT2;
    tyFX = tyFW = tyFV = (*pfnFunc0) ( tyX );
    tyDX = tyDW = tyDV = (*pfnFunc1) ( tyX );

    for (niCount = 1; niCount <= 100; ++niCount)
    {
        const TYPE                          tyTol1 = F(TgROOT_EPS) * F(tgPM_ABS)( tyX ) + F(TgEPS);
        const TYPE                          tyTol2 = MKL(2.0) * tyTol1;
        TYPE                                tyPrevD, tyPrevE;

        tyTM = MKL(0.5)*(tyA + tyB);

        if (F(tgPM_ABS)(tyX - tyTM) <= (tyTol2 - MKL(0.5)*(tyB - tyA)))
        {
            *ptyT0 = tyX;
            *ptyV0 = tyFX;
            return (TgTRUE);
        };

        tyPrevD = tyD;
        tyPrevE = tyE;

        tyD = MKL(0.5) * (tyE = F(tgPM_FSEL)( tyDX, tyA - tyX, tyB - tyX ));

        if (F(tgPM_ABS)(tyPrevE) - tyTol1 > MKL(0.0))
        {
            const TYPE tyD1 = F(tgPM_FSEL)(-F(tgPM_ABS)(tyDW-tyDX), (tyW-tyX)*tyDX/(tyDX-tyDW), MKL(2.0)*(tyB-tyA));
            const TYPE tyD2 = F(tgPM_FSEL)(-F(tgPM_ABS)(tyDV-tyDX), (tyV-tyX)*tyDX/(tyDX-tyDV), tyD1);
            const TYPE tyOK1 = F(tgPM_FSEL)((tyA-tyX-tyD1)*(tyX+tyD1-tyB), F(tgPM_FSEL)(-tyDX*tyD1,MKL(1.0),MKL(-1.0)),MKL(-1.0));
            const TYPE tyOK2 = F(tgPM_FSEL)((tyA-tyX-tyD2)*(tyX+tyD2-tyB), F(tgPM_FSEL)(-tyDX*tyD2,MKL(1.0),MKL(-1.0)),MKL(-1.0));

            const TYPE tyTMP = F(tgPM_FSEL)(
                tyOK1, F(tgPM_FSEL)( tyOK2, F(tgPM_FSEL)( F(tgPM_ABS)(tyD2) - F(tgPM_ABS)(tyD1), tyD1, tyD2 ), tyD1 ), tyD2
            );

            if ((tyOK1 + tyOK2 >= MKL(0.0)) && (F(tgPM_ABS)(MKL(0.5) * tyPrevE) - F(tgPM_ABS)(tyTMP) >= MKL(0.0)))
            {
                const TYPE                      tyNewD = F(tgPM_COPY_SIGN)(tyTol1, tyTM - tyX);

                tyE = tyPrevD;
                tyD = F(tgPM_FSEL)(tyA - tyX - tyTMP + tyTol2, tyNewD, F(tgPM_FSEL)(tyA - tyX - tyTMP + tyTol2, tyNewD, tyTMP));
            }
        }

        tyU = F(tgPM_FSEL)( F(tgPM_ABS)(tyD) - tyTol1, tyX + tyD, tyX + F(tgPM_COPY_SIGN)(tyTol1, tyD) );
        tyFU = (*pfnFunc0) (tyU);

        if (tyFU <= tyFX)
        {
            tyDU = (*pfnFunc1) (tyU);

            tyA = F(tgPM_FSEL)( tyU - tyX, tyX, tyA);
            tyB = F(tgPM_FSEL)( tyU - tyX, tyB, tyX);

            tyV = tyW;
            tyFV = tyFW;
            tyDV = tyDW;

            tyW = tyX;
            tyFW = tyFX;
            tyDW = tyDX;

            tyX = tyU;
            tyFX = tyFU;
            tyDX = tyDU;
        }
        else
        {
            if (F(tgPM_ABS)(tyD) < tyTol1)
            {
                *ptyT0 = tyX;
                *ptyV0 = tyFX;
                return (TgTRUE);
            }

            tyA = F(tgPM_FSEL)( tyX - tyU, tyU, tyA);
            tyB = F(tgPM_FSEL)( tyX - tyU, tyB, tyU);

            tyDU = (*pfnFunc1) (tyU);

            if (tyFU <= tyFW || tyW == tyX)
            {
                tyV = tyW;
                tyFV = tyFW;
                tyDV = tyDW;

                tyW = tyU;
                tyFW = tyFU;
                tyDW = tyDU;
            }
            else if (tyFU < tyFV || tyV == tyX || tyV == tyW)
            {
                tyV = tyU;
                tyFV = tyFU;
                tyDV = tyDU;
            }
        }
    }

    return (TgFALSE);
}


// ---- Degree 1 Algebraic Solver ----------------------------------------------------------------------------------------------- //
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL V(F_Calc_Root,1)( PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE tyC0, TYPE tyC1 )
{
    if (F(tgPM_ABS)(tyC1) >= F(TgROOT_EPS))
    {
        atyRoot[0] = -(tyC0 / tyC1);
        *piCount = 1;
        return (TgTRUE);
    }
    else
    {
        *piCount = 0;
        return (TgFALSE);
    };
}


// ---- Degree 2 Algebraic Solver ----------------------------------------------------------------------------------------------- //
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL V(F_Calc_Root,2)( PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE tyC0, TYPE tyC1, TYPE tyC2 )
{
    if (F(tgPM_ABS)(tyC2) <= F(TgROOT_EPS))
    {
        return (V(F_Calc_Root,1)( atyRoot, piCount, tyC0, tyC1 ));
    }
    else
    {
        TYPE                                tyDSC = tyC1*tyC1 - MKL(4.0)*tyC0*tyC2;
        TYPE                                tyTMP00;

        if (F(tgPM_ABS)(tyDSC) <= F(TgROOT_EPS))
        {
            tyDSC = MKL(0.0);
        };

        if (tyDSC < MKL(0.0))
        {
            *piCount = 0;
            return (TgFALSE);
        };

        tyTMP00 = MKL(0.5) / tyC2;

        if (tyDSC > MKL(0.0))
        {
            tyDSC = F(tgPM_SQRT)(tyDSC);
            
            atyRoot[0] = tyTMP00*(-tyC1 - tyDSC);
            atyRoot[1] = tyTMP00*(-tyC1 + tyDSC);
            
            *piCount = 2;
        }
        else
        {
            atyRoot[0] = -tyTMP00*tyC1;
            atyRoot[1] = -tyTMP00*tyC1;
            
            *piCount = 1;
        }

        return (TgTRUE);
    };
}


// ---- Degree 3 Algebraic Solver ----------------------------------------------------------------------------------------------- //
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL V(F_Calc_Root,3)( PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE tyC0, TYPE tyC1, TYPE tyC2, TYPE tyC3 )
{
    if (F(tgPM_ABS)( tyC3 ) <= F(TgEPS))
    {
        return (V(F_Calc_Root,2)( atyRoot, piCount, tyC0,tyC1,tyC2 ));
    };

    // Make polynomial monic, x^3+c2*x^2+c1*x+c0

    if (tyC3 != MKL(1.0))
    {
        const TYPE                          tyInvC3 = MKL(1.0) / tyC3;
        
        tyC0 *= tyInvC3;
        tyC1 *= tyInvC3;
        tyC2 *= tyInvC3;
    };

    // Convert to y^3+a*y+b = 0 by x = y-c2/3

    {
        const TYPE                          tyOffset = tyC2 / MKL(3.0);
        const TYPE                          tyA = tyC1 - tyC2*tyOffset;
        const TYPE                          tyTMP00 = tyC2*tyC2;
        const TYPE                          tyB = tyC0 + tyC2*(tyTMP00 + tyTMP00 - MKL(9.0)*tyC1)*(MKL(1.0) / MKL(27.0));
        const TYPE                          tyHalfB = MKL(0.5)*tyB;

        TYPE                                tyDSC = tyHalfB*tyHalfB + tyA*tyA*tyA*(MKL(1.0) / MKL(27.0));

        if (F(tgPM_ABS)(tyDSC) <= F(TgROOT_EPS))
        {
            tyDSC = MKL(0.0);
        };

        if (tyDSC > MKL(0.0))
        {
            const TYPE                          tyDSC_SQRT = F(tgPM_SQRT)( tyDSC );
            const TYPE                          tyTMP01 = -tyHalfB + tyDSC_SQRT;
            const TYPE                          tyTMP02 = -tyHalfB - tyDSC_SQRT;

            if (tyTMP01 >= MKL(0.0))
            {
                atyRoot[0] = F(tgPM_POW)( tyTMP01, MKL(1.0) / MKL(3.0) );
            }
            else
            {
                atyRoot[0] = -F(tgPM_POW)( -tyTMP01, MKL(1.0) / MKL(3.0) );
            }

            if (tyTMP02 >= MKL(0.0))
            {
                atyRoot[0] += F(tgPM_POW)( tyTMP02, MKL(1.0) / MKL(3.0) );
            }
            else
            {
                atyRoot[0] -= F(tgPM_POW)( -tyTMP02, MKL(1.0) / MKL(3.0) );
            }

            atyRoot[0] -= tyOffset;

            *piCount = 1;
        }
        else if ( tyDSC < MKL(0.0) )
        {

            const TYPE                          tyDist = F(tgPM_SQRT)( -tyA / MKL(3.0) );
            const TYPE                          tyAngle = F(tgPM_ATAN2)(F(tgPM_SQRT)(-tyDSC),-tyHalfB) / MKL(3.0);
            const TYPE                          tyCos = F(tgPM_COS)( tyAngle );
            const TYPE                          tySin = F(tgPM_SIN)( tyAngle );
            const TYPE                          tyTMP01 = tyDist*tyCos;

            atyRoot[0] = tyTMP01 + tyTMP01 - tyOffset;
            atyRoot[1] = -tyDist*( tyCos + F(TgKT_SQRT3)*tySin ) - tyOffset;
            atyRoot[2] = -tyDist*( tyCos - F(TgKT_SQRT3)*tySin ) - tyOffset;

            *piCount = 3;
        }
        else
        {
            TYPE                                tyTMP01;
            
            if (tyHalfB >= MKL(0.0))
            {
                tyTMP01 = -F(tgPM_POW)( tyHalfB, MKL(1.0) / MKL(3.0) );
            }
            else
            {
                tyTMP01 = F(tgPM_POW)( -tyHalfB, MKL(1.0) / MKL(3.0) );
            }

            atyRoot[0] = tyTMP01 + tyTMP01 - tyOffset;
            atyRoot[1] = -tyTMP01 - tyOffset;
            atyRoot[2] = atyRoot[1];

            *piCount = 3;
        }
    }

    return (TgTRUE);
}


// ---- Degree 4 Algebraic Solver ----------------------------------------------------------------------------------------------- //
// ------------------------------------------------------------------------------------------------------------------------------ //

TgBOOL V(F_Calc_Root,4)( PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE tyC0, TYPE tyC1, TYPE tyC2, TYPE tyC3, TYPE tyC4 )
{
    const TYPE                          tyInvC4 = MKL(1.0) / tyC4;

    if (F(tgPM_ABS)( tyC4 ) <= F(TgROOT_EPS))
    {
        return (V(F_Calc_Root,3)( atyRoot, piCount, tyC0, tyC1, tyC2, tyC3 ));
    }

    // Make polynomial monic, x^4+c3*x^3+c2*x^2+c1*x+c0

    if (tyInvC4 != MKL(1.0))
    {
        tyC0 *= tyInvC4;
        tyC1 *= tyInvC4;
        tyC2 *= tyInvC4;
        tyC3 *= tyInvC4;
    };

    {
        // Reduction to Resolvent Cubic Polynomial y^3+r2*y^2+r1*y+r0 = 0

        TYPE                                tyR0 = -tyC3*tyC3*tyC0 + MKL(4.0)*tyC0*tyC2 - tyC1*tyC1;
        TYPE                                tyR1 =  tyC3*tyC1 - MKL(4.0)*tyC0;
        TYPE                                tyR2 = -tyC2;
        TYPE                                tyDSC;

        V(F_Calc_Root,3)( atyRoot, piCount, tyR0, tyR1, tyR2, MKL(1.0) );

        tyDSC = MKL(0.25)*tyC3*tyC3 - tyC2 + atyRoot[0];

        *piCount = 0;

        if (F(tgPM_ABS)(tyDSC) <= F(TgROOT_EPS))
        {
            tyDSC = MKL(0.0);
        };

        if ( tyDSC > MKL(0.0) )
        {
            const TYPE                          tyTMP00 = F(tgPM_SQRT)(tyDSC);
            const TYPE                          tyTMP01 = MKL(0.75)*tyC3*tyC3 - tyTMP00*tyTMP00 - tyC2 - tyC2;
            const TYPE                          tyTMP02 = MKL(4.0)*(tyC3*tyC2 - tyC1 - tyC1);
            const TYPE                          tyTMP03 = MKL(0.25)*(tyTMP02  - tyC3*tyC3*tyC3) / (tyTMP00);

            TYPE                                tyTMP04 = tyTMP01 + tyTMP03;
            TYPE                                tyTMP05 = tyTMP01 - tyTMP03;

            if (F(tgPM_ABS)(tyTMP04) <= F(TgROOT_EPS))
            {
                tyTMP04 = MKL(0.0);
            }

            if (F(tgPM_ABS)(tyTMP05) <= F(TgROOT_EPS))
            {
                tyTMP05 = MKL(0.0);
            }

            if ( tyTMP04 >= MKL(0.0) )
            {
                const TYPE                          tyTMP06 = F(tgPM_SQRT)(tyTMP04);

                atyRoot[0] = MKL(-0.25)*tyC3 + MKL(0.5)*( tyTMP00 + tyTMP06 );
                atyRoot[1] = MKL(-0.25)*tyC3 + MKL(0.5)*( tyTMP00 - tyTMP06 );
                *piCount += 2;
            }

            if ( tyTMP05 >= MKL(0.0) )
            {
                const TYPE                          tyTMP06 = F(tgPM_SQRT)(tyTMP05);

                atyRoot[(*piCount)++] = MKL(-0.25)*tyC3 + MKL(0.5)*( tyTMP06 - tyTMP00 );
                atyRoot[(*piCount)++] = MKL(-0.25)*tyC3 - MKL(0.5)*( tyTMP06 + tyTMP00 );
            }

        }
        else if ( tyDSC < MKL(0.0) )
        {
            *piCount = 0;
        }
        else
        {
            TYPE                                tyTMP01 = atyRoot[0]*atyRoot[0] - MKL(4.0)*tyC0;

            if ( tyTMP01 >= -F(TgROOT_EPS) )
            {
                TYPE                                tyTMP00 = MKL(0.75)*tyC3*tyC3 - tyC2 - tyC2;

                tyTMP01 = tyTMP01 < MKL(0.0) ? MKL(0.0) : MKL(2.0)*F(tgPM_SQRT)(tyTMP01);

                if ( tyTMP00+tyTMP01 >= F(TgROOT_EPS) )
                {
                    TYPE                                tyTMP02 = F(tgPM_SQRT)(tyTMP00+tyTMP01);

                    atyRoot[0] = MKL(-0.25)*tyC3 + MKL(0.5)*tyTMP02;
                    atyRoot[1] = MKL(-0.25)*tyC3 - MKL(0.5)*tyTMP02;
                    *piCount += 2;
                }

                if ( tyTMP00-tyTMP01 >= F(TgROOT_EPS) )
                {
                    const TYPE                          tyTMP02= F(tgPM_SQRT)( tyTMP00-tyTMP01 );

                    atyRoot[(*piCount)++] = MKL(-0.25)*tyC3 + MKL(0.5)*tyTMP02;
                    atyRoot[(*piCount)++] = MKL(-0.25)*tyC3 - MKL(0.5)*tyTMP02;
                }
            }
        }
    }

    return *piCount > 0;
}